from __future__ import print_function

import copy
import math
import os
import sys
from collections import defaultdict
from operator import gt
from select import select
from time import sleep

import matplotlib.pyplot as plt
import numpy as np

# import matplotlib; matplotlib.use('Agg')

def readFile(path):
    file = open(path, "r")
    tra_list = file.readlines()
    test_file_5 = []
    for i in range(len(tra_list)):
        test_file_5.append(np.array(tra_list[i].strip().split(' '), dtype = np.float64 ))
    test_file_5 = np.array(test_file_5)
    file.close()
    return test_file_5

# test_file_1 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927/testing/trajectory/test.log")
# test_file_2 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_2/testing/trajectory/test.log")
# test_file_3 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_3/testing/trajectory/test.log")
# test_file_4 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_4/testing/trajectory/test.log")
# test_file_5 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_5/testing/trajectory/test.log")

# test_file_1 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927/testing/trajectory/test_abs.log")
# test_file_2 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_2/testing/trajectory/test_abs.log")
# test_file_3 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_3/testing/trajectory/test_abs.log")
# test_file_4 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_4/testing/trajectory/test_abs.log")
# test_file_5 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_5/testing/trajectory/test_abs.log")

# test_file_6 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/test_img.log")


# for i in range(x_data.shape[0]):
#     if(x_data[i]<2):
#         y_data[i] = 0.6
#     else:
#         y_data[i] = x_data[i]*0.2 + 0.2


# l1, =plt.plot(brown_gt[:,0], brown_gt[:,1], 'b') #v x
# l2, =plt.plot(brown_gt[:,0], brown_gt[:,8], 'r-.') #v x
# plt.legend(handles=[l1],labels=['Vx_estimated'])

# test_combine_data = np.vstack((test_file_4,test_file_5))
# split1_choose = test_combine_data[:,6] == 1
# split1 = test_combine_data[split1_choose,0:6]
# split1_choose = test_combine_data[:,6] == 2
# split2 = test_combine_data[split1_choose,0:6]
# split1_choose = test_combine_data[:,6] == 3
# split3 = test_combine_data[split1_choose,0:6]
# split1_choose = test_combine_data[:,6] == 4
# split4 = test_combine_data[split1_choose,0:6]

# plt.xlabel('Vx(m/s)')
# plt.ylabel('Error(m)')
# plt.scatter(test_file_1[:,0], test_file_1[:,3])
# plt.scatter(test_file_2[:,0], test_file_2[:,3])
# plt.scatter(test_file_3[:,0], test_file_3[:,3])
# plt.scatter(test_file_4[:,0], test_file_4[:,3])
# plt.scatter(test_file_5[:,0], test_file_5[:,3])
# l1 = plt.scatter(split1[:,0], split1[:,3])
# l2 = plt.scatter(split2[:,0], split2[:,3])
# l3 = plt.scatter(split3[:,0], split3[:,3])
# l4 = plt.scatter(split4[:,0], split4[:,3])
# plt.legend(handles=[l1,l2,l3,l4],labels=['1t', '2t', '3t', '4t'])

# plt.xlabel('Vy(m/s)')
# plt.ylabel('Error(m)')
# plt.scatter(test_file_1[:,1], test_file_1[:,4])
# plt.scatter(test_file_2[:,1], test_file_2[:,4])
# plt.scatter(test_file_3[:,1], test_file_3[:,4])
# plt.scatter(test_file_4[:,1], test_file_4[:,4])
# plt.scatter(test_file_5[:,1], test_file_5[:,4])
# l1 = plt.scatter(split1[:,1], split1[:,4])
# l2 = plt.scatter(split2[:,1], split2[:,4])
# l3 = plt.scatter(split3[:,1], split3[:,4])
# l4 = plt.scatter(split4[:,1], split4[:,4])
# plt.legend(handles=[l1,l2,l3,l4],labels=['1t', '2t', '3t', '4t'])

# plt.xlabel('Vz(m/s)')
# plt.ylabel('Error(m)')
# plt.scatter(test_file_1[:,1], test_file_1[:,4])
# plt.scatter(test_file_2[:,1], test_file_2[:,4])
# plt.scatter(test_file_3[:,1], test_file_3[:,4])
# plt.scatter(test_file_4[:,1], test_file_4[:,4])
# plt.scatter(test_file_5[:,1], test_file_5[:,4])
# l1 = plt.scatter(split1[:,2], split1[:,5])
# l2 = plt.scatter(split2[:,2], split2[:,5])
# l3 = plt.scatter(split3[:,2], split3[:,5])
# l4 = plt.scatter(split4[:,2], split4[:,5])
# plt.legend(handles=[l1,l2,l3,l4],labels=['1t', '2t', '3t', '4t'])
# plt.xlim(-0.1,1.)
# plt.ylim(-0.65,0.65)

# split1_choose = test_file_6[:,2] == 1
# split1 = test_file_6[split1_choose,0:2]
# split1_choose = test_file_6[:,2] == 2
# split2 = test_file_6[split1_choose,0:2]
# split1_choose = test_file_6[:,2] == 3
# split3 = test_file_6[split1_choose,0:2]
# split1_choose = test_file_6[:,2] == 4
# split4 = test_file_6[split1_choose,0:2]
# split1_choose = test_file_6[:,2] == 5
# split5 = test_file_6[split1_choose,0:2]
#
# plt.scatter(split1[:,0], split1[:,1])
# plt.scatter(split2[:,0], split2[:,1])
# plt.scatter(split3[:,0], split3[:,1])
# plt.scatter(split4[:,0], split4[:,1])
# plt.scatter(split5[:,0], split5[:,1])
# plt.scatter(test_file_6[:,2],test_file_6[:,1])
# plt.xlim(-0.2,5)
# plt.ylim(-2.,2.)

# plt.plot(x_data,y_data,"r")
# plt.show()




# img
# test_file_4 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_4/testing/trajectory/test_img.log")
# test_file_5 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_5/testing/trajectory/test_img.log")
# test_combine_data = np.vstack((test_file_4,test_file_5))
# split1_choose = test_combine_data[:,2] == 1
# split1 = test_combine_data[split1_choose,0:2]
# split1_choose = test_combine_data[:,2] == 2
# split2 = test_combine_data[split1_choose,0:2]
# split1_choose = test_combine_data[:,2] == 3
# split3 = test_combine_data[split1_choose,0:2]
# split1_choose = test_combine_data[:,2] == 4
# split4 = test_combine_data[split1_choose,0:2]
# plt.xlabel('V(m/s)')
# plt.ylabel('Error(pixel)')
# l1 = plt.scatter(split1[:,0], split1[:,1])
# l2 = plt.scatter(split2[:,0], split2[:,1])
# l3 = plt.scatter(split3[:,0], split3[:,1])
# l4 = plt.scatter(split4[:,0], split4[:,1])
# plt.legend(handles=[l1,l2,l3,l4],labels=['1t', '2t', '3t', '4t'])
#
# plt.savefig("./error_vx.png")
# plt.show()

# pre abs
test_file_4 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_4/testing/trajectory/test_pre_abs_zero.log")
test_file_5 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_5/testing/trajectory/test_pre_abs_zero.log")
test_combine_data = np.vstack((test_file_4,test_file_5))
split1_choose = test_combine_data[:,6] == 1
split1 = test_combine_data[split1_choose,0:6]
split1_choose = test_combine_data[:,6] == 2
split2 = test_combine_data[split1_choose,0:6]
split1_choose = test_combine_data[:,6] == 3
split3 = test_combine_data[split1_choose,0:6]
split1_choose = test_combine_data[:,6] == 4
split4 = test_combine_data[split1_choose,0:6]
# x_data = np.arange(0.0,5.0,0.1)
# y_data1 = x_data*0.09 + 0.4
# y_data2 = -x_data*0.09 - 0.4

plt.xlabel('Vx(m/s)')
plt.ylabel('Error(m)')
l1 = plt.scatter(split1[:,0], split1[:,3])
l2 = plt.scatter(split2[:,0], split2[:,3])
l3 = plt.scatter(split3[:,0], split3[:,3])
l4 = plt.scatter(split4[:,0], split4[:,3])
plt.legend(handles=[l1,l2,l3,l4],labels=['1t', '2t', '3t', '4t'])
# plt.plot(x_data,y_data1,"r")
# plt.plot(x_data,y_data2,"r")
plt.xlim(-0.1,5)
plt.ylim(-1.,1.)
plt.savefig("./error_vx.png")
plt.show()
# plt.xlabel('Vy(m/s)')
# plt.ylabel('Error(m)')
# l1 = plt.scatter(split1[:,1], split1[:,4])
# l2 = plt.scatter(split2[:,1], split2[:,4])
# l3 = plt.scatter(split3[:,1], split3[:,4])
# l4 = plt.scatter(split4[:,1], split4[:,4])
# plt.legend(handles=[l1,l2,l3,l4],labels=['1t', '2t', '3t', '4t'])
# # plt.plot(x_data,y_data,"r")
# plt.xlim(-0.1,5)
# plt.ylim(-1.,1.)
# plt.savefig("./error_vx.png")
# plt.show()
# plt.xlabel('Vz(m/s)')
# plt.ylabel('Error(m)')
# l1 = plt.scatter(split1[:,2], split1[:,5])
# l2 = plt.scatter(split2[:,2], split2[:,5])
# l3 = plt.scatter(split3[:,2], split3[:,5])
# l4 = plt.scatter(split4[:,2], split4[:,5])
# plt.legend(handles=[l1,l2,l3,l4],labels=['1t', '2t', '3t', '4t'])
# # plt.plot(x_data,y_data,"r")
# plt.xlim(-0.1,1)
# plt.ylim(-1.,1.)
# plt.savefig("./error_vx.png")
# plt.show()

# pc img
# test_file_4 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_4/testing/trajectory/test_pc_img.log")
# test_file_5 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/0927_5/testing/trajectory/test_pc_img.log")
# test_combine_data = np.vstack((test_file_4,test_file_5))
# plt.xlabel('frame')
# plt.ylabel('Error(pixel)')
# l1 = plt.scatter(test_combine_data[:,0], test_combine_data[:,1])
# plt.savefig("./error_vx.png")
# plt.show()

# cost time
# test_file_4 = readFile("/home/st/ubuntu_data/dataset/gazebo/envalute/cost_time.log")
# plt.xlabel('frame')
# plt.ylabel('cost time(ms)')
# l1 = plt.scatter(test_file_4[:,0], test_file_4[:,1])
# print(np.mean(test_file_4[:,1]))
# plt.savefig("./error_vx.png")
# plt.show()